#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <vision/RF.h>

namespace enc = sensor_msgs::image_encodings;

class Retina {

protected:
  ros::NodeHandle n_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;

  ros::Publisher rf_values_pub_;
  std::vector<std::pair<int, int> > rf_locs_;

 // Load the W,H points of the receptive fields,
  // from external source or programatically
  bool load_rf_locs(void) {

    // Values/Method here for testing only 
    int H = 240;
    int W = 320;

    int v_space = 10;
    int h_space = 15;

    for(int w = 10; w < W; w += h_space)
      for(int h = 80; h < 160; h += v_space)
      {
	  std::pair<int,int> p(w,h);
	  rf_locs_.push_back(p);
      }
    return true;
  }

public:
  
  Retina(ros::NodeHandle &n) :
    n_(n), it_(n_)
  {
    load_rf_locs();

    image_pub_ = it_.advertise("/rika/front_cam/retina_image", 1);
    rf_values_pub_ = n_.advertise<vision::RF>("/rika/front_cam/retina_image/rf_values", 1);

    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, 
			       &Retina::process, this);

  }

  ~Retina()
  {

  }

  void process(const sensor_msgs::ImageConstPtr& msg_ptr)
  {
    typedef struct elem_ { float val[3];    } elem;

    float LoG[5][5] = {
      {0.0448,    0.0468,    0.0564,    0.0468,    0.0448},
      {0.0468,    0.3167,    0.7146,    0.3167,    0.0468},
      {0.0564,    0.7146,   -4.9048,    0.7146,    0.0564},
      {0.0468,    0.3167,    0.7146,    0.3167,    0.0468},
      {0.0448,    0.0468,    0.0564,    0.0468,    0.0448}
    };
    float canny[3][3] =  {{0, 1, 0}, {1, -4, 1}, {0, 1, 1}};
    float null_filter[3][3] =  {{0, 0, 0}, {0, 1, 0}, {0, 0, 0}};

    cv::Mat LoG_filter = cv::Mat(5,5,CV_32F, LoG);
    cv::Mat canny_filter = cv::Mat(3,3,CV_32F, canny);
    cv::Mat null = cv::Mat(3,3,CV_32F, null_filter);
    
    /////////////////////////

    cv_bridge::CvImagePtr cv_ptr;
    try
      {
	cv_ptr = cv_bridge::toCvCopy(msg_ptr, enc::MONO8);
      }
    catch (cv_bridge::Exception& e)
      {
	ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
      }

    cv::filter2D(cv_ptr->image, cv_ptr->image, -1, LoG_filter, cv::Point(-1, -1), 0.0, cv::BORDER_REPLICATE);

    // Publish filtered image
    try
      {
	image_pub_.publish(cv_ptr->toImageMsg());
      }
    catch(cv_bridge::Exception& e)
      {
	ROS_ERROR("Error publishing filtered image");
      }
  

    ////////////////////////////
    // RF value extraction
    //
    vision::RF rf_vals;
    int num_pts = rf_locs_.size();

    for(int i = 0; i < num_pts; i++)
      {
	vision::RF_value rfv;
	rfv.x_disp = rf_locs_[i].first;
	rfv.y_disp = rf_locs_[i].second;
	rfv.index = i;

	unsigned short pxl = cv_ptr->image.at<unsigned short>(rf_locs_[i].first,rf_locs_[i].second);

	rfv.val = (float)pxl/(float)USHRT_MAX;
	if(rfv.val > 0.01)
	  rf_vals.rf.push_back(rfv);

	//ROS_INFO("%d %d: %f", rf_locs_[i].first,rf_locs_[i].second, rfv.val);

      }

    // Publish RF values
    try
      {
	rf_values_pub_.publish(rf_vals);
      }
    catch(std::exception e)
      {
	ROS_ERROR("Error publishing rf values");
      }

  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "receptive_field");
  ros::NodeHandle n;
  Retina rf(n);
  ros::spin();
  return 0;
}
